function [lat2, lon2, rev_az] = vreckon(lat, lon, dist_km, az)
%   Computes the coordinates from a point towards a bearing at given distances.
%
%   This uses Vincenty formula with an accuracy parameter used
%   to set the convergence of lambda. The default 1E-12 corresponds
%   to approximately 0.06 mm.
%
%   The formulas and nomenclature are from Vincenty, 1975:
%     https://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
%   See also:
%     https://en.wikipedia.org/wiki/Vincenty's_formulae
%
%   Inputs:
%     lat,lon: the initial point coordinates (in degrees),
%     dist_km: the distance of the target point (in km)
%     bearing: the bearing angle (in degrees)
%     accuracy: accuracy for the vincenty convergence (optional)
%
%   Returns:
%     lat2, lon2, A21 point latitudes, longitudes and reverse bearings, all in degrees.

% Input validation
if ~isnumeric(lat) || ~isnumeric(lon) || ~isnumeric(dist_km) || ~isnumeric(az)
    error('All inputs must be numeric');
end

% Ensure scalar inputs are expanded to match vector inputs
if isscalar(lat) && ~isscalar(lon)
    lat = repmat(lat, size(lon));
end
if isscalar(lon) && ~isscalar(lat)
    lon = repmat(lon, size(lat));
end
if isscalar(dist_km) && ~isscalar(lat)
    dist_km = repmat(dist_km, size(lat));
end
if isscalar(az) && ~isscalar(lat)
    az = repmat(az, size(lat));
end

% Check input sizes
if ~isequal(size(lat), size(lon), size(dist_km), size(az))
    error('All inputs must have the same size or be scalar');
end

% Convert to column vectors for consistency
lat = lat(:);
lon = lon(:);
dist_km = dist_km(:);
az = az(:);

% Parameters
accuracy = 1.0E-12;
a = 6378.1370;        % semi-major axis (km), WGS84
f = 1/298.257223563;  % flattening of the ellipsoid, WGS84
b = (1-f)*a;          % semi-minor axis (m)

% Convert inputs to double precision for calculation
phi1 = double(lat);
L1   = double(lon);
alpha1 = double(az);
s = double(dist_km);

% Precompute trigonometric values
sin_alpha1 = sin(alpha1);
cos_alpha1 = cos(alpha1);

% Compute reduced latitude
tan_u1 = (1-f) * tan(phi1);
cos_u1 = 1 ./ sqrt(1 + tan_u1.^2);
sin_u1 = tan_u1 .* cos_u1;

sigma1 = atan2(tan_u1, cos_alpha1);
sin_alpha = cos_u1 .* sin_alpha1;
cos2_alpha = 1 - sin_alpha.^2;
u2 = cos2_alpha .* (a^2 - b^2) / b^2;

% Compute Vincenty coefficients
A = 1 + u2/16384 .* (4096 + u2.*(-768 + u2.*(320 - 175*u2)));
B = u2/1024 .* (256 + u2.*(-128 + u2.*(74 - 47*u2)));

% Initialize sigma
sigma = s ./ (b .* A);
sigma_prev = inf(size(sigma));

% Iterate until convergence
max_iter = 100;
iter = 0;
not_converged = true(size(sigma));

while any(not_converged) && iter < max_iter
    cos_2sigma_m = cos(2*sigma1(not_converged) + sigma(not_converged));
    sin_sigma = sin(sigma(not_converged));
    cos_sigma = cos(sigma(not_converged));
    
    delta_sigma = B(not_converged) .* sin_sigma .* (cos_2sigma_m + B(not_converged)/4 .* ...
                 (cos_sigma .* (-1 + 2*cos_2sigma_m.^2) - B(not_converged)/6 .* cos_2sigma_m .* ...
                 (-3 + 4*sin_sigma.^2) .* (-3 + 4*cos_2sigma_m.^2)));
    
    sigma_prev(not_converged) = sigma(not_converged);
    sigma(not_converged) = s(not_converged) ./ (b .* A(not_converged)) + delta_sigma;
    
    % Check convergence
    not_converged(not_converged) = abs(sigma(not_converged) - sigma_prev(not_converged)) > accuracy;
    iter = iter + 1;
end

if iter == max_iter && any(not_converged)
    warning('Vincenty formula did not converge for some points');
end

% Compute final values
cos_2sigma_m = cos(2*sigma1 + sigma);
cos_sigma = cos(sigma);
sin_sigma = sin(sigma);

tmp = sin_u1 .* sin_sigma - cos_u1 .* cos_sigma .* cos_alpha1;
phi2 = atan2(sin_u1 .* cos_sigma + cos_u1 .* sin_sigma .* cos_alpha1, ...
             (1-f) * sqrt(sin_alpha.^2 + tmp.^2));

lambda = atan2(sin_sigma .* sin_alpha1, ...
               cos_u1 .* cos_sigma - sin_u1 .* sin_sigma .* cos_alpha1);

C = f/16 .* cos2_alpha .* (4 + f*(4-3*cos2_alpha));
L = lambda - (1-C) .* f .* sin_alpha .* ...
    (sigma + C .* sin_sigma .* (cos_2sigma_m + C .* cos_sigma .* ...
    (-1 + 2 * cos_2sigma_m.^2)));

L2 = L1 + L;

% Normalize longitude to [-pi, pi]
% lon2 = single(mod(L2 + pi, 2*pi) - pi);
% lat2 = single(phi2);
lon2 = mod(L2 + pi, 2*pi) - pi;
lat2 = phi2;

% Calculate reverse azimuth
rev_az = atan2(sin_alpha, -tmp);
% rev_az = single(mod(rev_az + pi, 2*pi) - pi);
rev_az = mod(rev_az + pi, 2*pi) - pi;
end